#include "RenderUtils.h"

int delta = 0;
void *font = GLUT_BITMAP_TIMES_ROMAN_24;
char s[1000];

char buff1[8];
void SaveAllBuffers(int w, int h, int num) {

	int thesize = sizeof(char) * w * h * 3;
	char* pixels = (char*)malloc( thesize );
	CvMat* img = cvCreateMat(h,w,CV_8UC3);

	int initWindowID = glutGetWindow();
	for( int idx = 0; idx < num; idx++ ) {
		_itoa_s(idx, buff1,8, 10 );
		string end(buff1);
		glutSetWindow(idx+1);
		glReadPixels( 0, 0, w, h, GL_RGB, GL_UNSIGNED_BYTE, pixels );
		int k = 0;
		for( int i = h-1; i >= 0; i-- ) {
			for( int j = 0; j < w; j++ ) {
				uchar r = (uchar) pixels[k];
				uchar g = (uchar) pixels[k+1];
				uchar b = (uchar) pixels[k+2];
				CvScalar scalar = cvScalar(b,g,r);
				cvSet2D(img,i,j,scalar);
				k+=3;
			}
		}
		string name = "ReadBufferWindow";
		name.append(end);
		string fname = ".png";
		name.append(fname);
		cvSaveImage( name.c_str(), img );
		glFinish();
		int breakhere = 1;

	}
	cvReleaseMat( &img );
	free( pixels );
	glutSetWindow(initWindowID);

}

void TestReadBuffer(int h, int w)
{
	int thesize = sizeof(char) * w * h * 3;
	char* pixels = (char*)malloc( thesize );
	glReadPixels( 0, 0, w, h, GL_RGB, GL_UNSIGNED_BYTE, pixels );
	IplImage* img = cvCreateImage( cvSize(w,h), IPL_DEPTH_8U, 3 );
	cvZero(img);
	int step       = img->widthStep/sizeof(uchar);
	int channels   = img->nChannels;
	uchar* data    = (uchar *)img->imageData;
	
	for( int k = 0; k < thesize; k+=3 ) {
		int i = h - k / 3 / h - 1;
		int j = (k / 3) % h;
		int r = (int) pixels[k];
		int g = (int) pixels[k+1];
		int b = (int) pixels[k+2];
		data[i*step+j*channels+0]=b; // B
		data[i*step+j*channels+1]=g; // G
		data[i*step+j*channels+2]=r; // R
		int breakehre = 1;
	}
	cvSaveImage( "TestReadBuffer.png", img );
	cvReleaseImage( &img );
	free( pixels );
	int breakhere = 1;
}

void DrawAxes(vector<float>& size)
{
	glPushMatrix();
	// Draw and Label Axes
	glScalef(size[0],size[1],size[2]);
    glColor3f (1.0, 1.0, 1.0);
	glBegin(GL_LINES);
                glVertex3f(-1.8f, 0.0f,0.0f);
                glVertex3f(1.8f, 0.0f,0.0f);
    glEnd();
    glBegin(GL_LINES);
                glVertex3f(0.0f, -1.8f,0.0f);
                glVertex3f(0.0f, 1.8f,0.0f);
    glEnd();
    glBegin(GL_LINES);
                glVertex3f(0.0f, 0.0f, -1.8f);
                glVertex3f(0.0f, 0.0f, 1.8f);
    glEnd();

    sprintf(s,"+z");
    outputCharacter(0.0f, 0.0f, 1.98f, s);
    sprintf(s,"-z");
    outputCharacter(0.0f, 0.0f, -1.98f, s);
    sprintf(s,"+x");
    outputCharacter(1.9f, 0.0f, 0.0f, s);
    sprintf(s,"-x");
    outputCharacter(-1.9f, 0.0f, 0.0f, s);
    sprintf(s,"+y");
    outputCharacter(0.0f, 1.9f, 0.0f, s);
    sprintf(s,"-y");
    outputCharacter(0.0f, -1.99f, 0.0f, s);
	glPopMatrix();
}

void DrawCalibrationPoints(const vector<float>& ptsxyz) {
	glPushMatrix();
	glColor3f (1.0, 0.0, 1.0);
	
	int len = ptsxyz.size();

	for( int i = 0; i < len; i+=3 ) {
		sprintf(s,"*");
		float ptx = ptsxyz[i];
		float pty = ptsxyz[i+1];
		float ptz = ptsxyz[i+2];
		outputCharacter(ptx, pty, ptz, s);
	}
	glPopMatrix();
}

void outputCharacter(float x, float y, float z, char *string) {
  int len, i;
  glRasterPos3f(x, y, z);
  len = (int) strlen(string);
  for (i = 0; i < len; i++) {
    glutBitmapCharacter(font, string[i]);
  }
}

char buff[32];
char frm[8];
IplImage* ReadImgByNum(const string& basename, const string& end, int iFrame, int numChars) {
	
		
	_itoa_s( iFrame, buff, 20 );
	int num = 0; // number of nonzero entries
	char val = buff[num];
	while( val != 0 && num < numChars) {
		num++;
		val = buff[num];
	}
	int idx = 0;
	for( int i = 0; i < numChars; i++ ) { frm[i] = '0'; }
	for( int i = numChars-num; i<numChars; i++) { 
		frm[i] = buff[i-numChars+num]; 
	}	
	string csFrame;
	csFrame.assign( frm, numChars );

	string filename = basename;
	filename.append(csFrame);
	filename.append(end);

	IplImage* img = cvLoadImage( filename.c_str() );
	return img;

}